#include "motiondetection.h"

#include <stdexcept>
#include <stdio.h>

using namespace cv;
using namespace std;

int maxThreshold = 40;

#define MAX_DEPTH 2048
#define MAX_RGB 256

MotionDetection::MotionDetection( int imgWidth , int imgHeight ,
                                  BackGroundModel *bgGenRGB , BackGroundModel *bgGenDepth )
{
    changeRGB = Mat::zeros(imgHeight, imgWidth, CV_8UC1);
    changeMot = Mat::zeros(imgHeight, imgWidth, CV_8UC1);

    this->bgGenRGB = bgGenRGB;
    this->bgGenDepth = bgGenDepth;

    frameCount = 0;
    mode = 0;
}

// TODO compute threshold separately for motion image
bool MotionDetection::addFrame( const cv::Mat &imageRGB , const cv::Mat &imageDepth )
{

//    printf("Motion Detection Mode %d\n",mode);

    switch( mode ) {
    case 0:
        bgGenRGB->reset();
        bgGenRGB->addImage(imageRGB);

        bgGenDepth->reset();
        bgGenDepth->addImage(imageDepth);
        mode = 1;
        return false;

    case 1:
        {
            bool retRGB = bgGenRGB->addImage(imageRGB);
            bool retDepth = bgGenDepth->addImage(imageDepth);

            if( retRGB && retDepth ) {
                bgGenRGB->computeModel();
                bgGenDepth->computeModel();
                mode = 2;
                frameCount = 0;
            }

            printf("frame count = %d\n",frameCount);
            return false;
        }

    case 2:
        bgGenRGB->findMovement(imageRGB,changeRGB);
        bgGenDepth->findMovement(imageDepth,changeMot);
        return true;
    }

    throw std::runtime_error("bug");
}

int MotionDetection::getMode()
{
    return mode;
}

std::string MotionDetection::getStatus()
{
    switch( mode ) {
    case 0:
        return "Initialize";

    case 1:
        return "Creating background model";

    case 2:
        return "Creating difference image";
    }

    throw std::runtime_error("Unknown mode");
}

cv::Mat& MotionDetection::getBackgroundRGB()
{
    return bgGenRGB->getBackground();
}

cv::Mat& MotionDetection::getMotionRGB()
{
    return changeRGB;
}

cv::Mat& MotionDetection::getBackgroundDepth()
{
    return bgGenDepth->getBackground();
}

cv::Mat& MotionDetection::getMotionDepth()
{
    return changeMot;
}

